#include <fstream>
#include <iostream>
#include <iomanip>
#include <thread>
#include <mutex>

using namespace std;
using namespace std::placeholders;

#include "dso/FullSystem/FullSystem.h"
#include "dso/FullSystem/HessianBlocks.h"
#include "dso/FullSystem/CoarseTracker.h"
#include "dso/FullSystem/ImmaturePoint.h"
#include "dso/FullSystem/PixelSelector.h"
#include "dso/FullSystem/CoarseInitializer.h"

#include "dso/OptimizationBackend/EnergyFunctional.h"
#include "dso/OptimizationBackend/EnergyFunctionalStructs.h"

#include "dso/util/Settings.h"
#include "dso/util/FrameShell.h"
#include "dso/util/MinimalImage.h"

#include "dso/IOWrapper/ImageRW.h"
#include "dso/IOWrapper/ImageDisplay.h"
#include "dso/IOWrapper/Output3DWrapper.h"


namespace dso
{

// constructor
FullSystem::FullSystem()
{
    // 建立系统，新建各种跟踪、建图对象
    int retstat =0;
    if ( setting_logStuff )
    {
        // open the log files
        retstat += system ( "rm -rf logs" );
        retstat += system ( "mkdir logs" );

        retstat += system ( "rm -rf mats" );
        retstat += system ( "mkdir mats" );

        calibLog = new std::ofstream();
        calibLog->open ( "logs/calibLog.txt", std::ios::trunc | std::ios::out );
        calibLog->precision ( 12 );

        numsLog = new std::ofstream();
        numsLog->open ( "logs/numsLog.txt", std::ios::trunc | std::ios::out );
        numsLog->precision ( 10 );

        coarseTrackingLog = new std::ofstream();
        coarseTrackingLog->open ( "logs/coarseTrackingLog.txt", std::ios::trunc | std::ios::out );
        coarseTrackingLog->precision ( 10 );

        eigenAllLog = new std::ofstream();
        eigenAllLog->open ( "logs/eigenAllLog.txt", std::ios::trunc | std::ios::out );
        eigenAllLog->precision ( 10 );

        eigenPLog = new std::ofstream();
        eigenPLog->open ( "logs/eigenPLog.txt", std::ios::trunc | std::ios::out );
        eigenPLog->precision ( 10 );

        eigenALog = new std::ofstream();
        eigenALog->open ( "logs/eigenALog.txt", std::ios::trunc | std::ios::out );
        eigenALog->precision ( 10 );

        DiagonalLog = new std::ofstream();
        DiagonalLog->open ( "logs/diagonal.txt", std::ios::trunc | std::ios::out );
        DiagonalLog->precision ( 10 );

        variancesLog = new std::ofstream();
        variancesLog->open ( "logs/variancesLog.txt", std::ios::trunc | std::ios::out );
        variancesLog->precision ( 10 );


        nullspacesLog = new std::ofstream();
        nullspacesLog->open ( "logs/nullspacesLog.txt", std::ios::trunc | std::ios::out );
        nullspacesLog->precision ( 10 );
    }
    else
    {
        // don't use log files
    }

    assert ( retstat!=293847 ); // ???

    // create the objects
    selectionMap = new float[wG[0]*hG[0]];
    coarseDistanceMap = new CoarseDistanceMap ( wG[0], hG[0] );
    coarseTracker = new CoarseTracker ( wG[0], hG[0] );
    coarseTracker_forNewKF = new CoarseTracker ( wG[0], hG[0] );
    coarseInitializer = new CoarseInitializer ( wG[0], hG[0] );
    pixelSelector = new PixelSelector ( wG[0], hG[0] );

    lastCoarseRMSE.setConstant ( 100 );

    ef = new EnergyFunctional();
    ef->red = &this->treadReduce;

    mappingThread = std::thread ( &FullSystem::mappingLoop, this );
}

FullSystem::~FullSystem()
{
    // delete the stuff in constructor
    blockUntilMappingIsFinished();

    if ( setting_logStuff )
    {
        calibLog->close();
        delete calibLog;
        numsLog->close();
        delete numsLog;
        coarseTrackingLog->close();
        delete coarseTrackingLog;
        //errorsLog->close(); delete errorsLog;
        eigenAllLog->close();
        delete eigenAllLog;
        eigenPLog->close();
        delete eigenPLog;
        eigenALog->close();
        delete eigenALog;
        DiagonalLog->close();
        delete DiagonalLog;
        variancesLog->close();
        delete variancesLog;
        nullspacesLog->close();
        delete nullspacesLog;
    }

    delete[] selectionMap;

    for ( FrameShell* s : allFrameHistory )
    {
        delete s;
    }
    for ( FrameHessian* fh : unmappedTrackedFrames )
    {
        delete fh;
    }

    delete coarseDistanceMap;
    delete coarseTracker;
    delete coarseTracker_forNewKF;
    delete coarseInitializer;
    delete pixelSelector;
    delete ef;
}

// 添加一个图像帧，跟踪并判断是否为关键帧
void FullSystem::addActiveFrame ( ImageAndExposure* image, int id )
{
    if ( isLost )   // lost
    {
        return;
    }

    std::unique_lock<std::mutex> lock ( trackMutex );

    // =========================== add into allFrameHistory =========================
    // 预处理 1. 新建 Frame 并加入到历史数据中
    FrameHessian* fh = new FrameHessian();
    FrameShell* shell = new FrameShell();
    shell->camToWorld = SE3();      // no lock required, as fh is not used anywhere yet.
    shell->aff_g2l = AffLight ( 0,0 );
    shell->marginalizedAt = shell->id = allFrameHistory.size();
    shell->timestamp = image->timestamp;
    shell->incoming_id = id;
    fh->shell = shell;
    allFrameHistory.push_back ( shell );

    // =========================== make Images / derivatives etc. =========================
    // 预处理 2. 建立图像(金字塔，梯度图)
    fh->ab_exposure = image->exposure_time;
    fh->makeImages ( image->image, &Hcalib );

    if ( !initialized )
    {
        // initialize the monocular slam
        // 初始化
        if ( coarseInitializer->frameID<0 ) // first frame set. fh is kept by coarseInitializer.
        {
            coarseInitializer->setFirst ( &Hcalib, fh );
        }
        else if ( coarseInitializer->trackFrame ( fh, outputWrapper ) ) // if SNAPPED
        {
            // 初始化成功
            initializeFromInitializer ( fh );
            deliverTrackedFrame ( fh, true );
        }
        else
        {
            // if still initializing
            fh->shell->poseValid = false;
            delete fh;
        }
        return;
    }
    else
    {
        // initialized, do front-end operation
        // 已经初始化，跟踪之
        if ( coarseTracker_forNewKF->refFrameID > coarseTracker->refFrameID )
        {
            std::unique_lock<std::mutex> crlock ( coarseTrackerSwapMutex );
            CoarseTracker* tmp = coarseTracker;
            coarseTracker=coarseTracker_forNewKF;
            coarseTracker_forNewKF=tmp;
        }

        Vec4 tres = trackNewCoarse ( fh );
        if ( !std::isfinite ( ( double ) tres[0] ) || !std::isfinite ( ( double ) tres[1] ) || !std::isfinite ( ( double ) tres[2] ) || !std::isfinite ( ( double ) tres[3] ) )
        {
            // Track is LOST
            std::cout << "Initial Tracking failed: LOST!\n" <<std::endl;
            isLost=true;
            return;
        }

        bool needToMakeKF = false;
        if ( setting_keyframesPerSecond > 0 ) // 是否需要定时插关键帧？
        {
            // 定时插新帧
            needToMakeKF = allFrameHistory.size() == 1 ||
                           ( fh->shell->timestamp - allKeyFramesHistory.back()->timestamp ) > 0.95f/setting_keyframesPerSecond;
        }
        else
        {
            // 判断是否要插新关键帧
            Vec2 refToFh=AffLight::fromToVecExposure ( coarseTracker->lastRef->ab_exposure, fh->ab_exposure,
                         coarseTracker->lastRef_aff_g2l, fh->shell->aff_g2l );

            // BRIGHTNESS CHECK
            needToMakeKF = allFrameHistory.size() == 1 ||
                           setting_kfGlobalWeight*setting_maxShiftWeightT *  sqrtf ( ( double ) tres[1] ) / ( wG[0]+hG[0] ) +
                           setting_kfGlobalWeight*setting_maxShiftWeightR *  sqrtf ( ( double ) tres[2] ) / ( wG[0]+hG[0] ) +
                           setting_kfGlobalWeight*setting_maxShiftWeightRT * sqrtf ( ( double ) tres[3] ) / ( wG[0]+hG[0] ) +
                           setting_kfGlobalWeight*setting_maxAffineWeight * fabs ( logf ( ( float ) refToFh[0] ) ) > 1 ||
                           2*coarseTracker->firstCoarseRMSE < tres[0];
        }

        for ( IOWrap::Output3DWrapper* ow : outputWrapper )
        {
            ow->publishCamPose ( fh->shell, &Hcalib );
        }

        deliverTrackedFrame ( fh, needToMakeKF );
        return;
    }
}

void FullSystem::deliverTrackedFrame ( FrameHessian* fh, bool needKF )
{
    if ( linearizeOperation )
    {
        // 默认需要做线性化处理
        // GUI operations ，是否步进/退出等功能
        if ( goStepByStep && lastRefStopID != coarseTracker->refFrameID )
        {
            MinimalImageF3 img ( wG[0], hG[0], fh->dI );
            IOWrap::displayImage ( "frameToTrack", &img );
            while ( true )
            {
                char k=IOWrap::waitKey ( 0 );
                if ( k==' ' )
                {
                    break;
                }
                handleKey ( k );
            }
            lastRefStopID = coarseTracker->refFrameID;
        }
        else
        {
            handleKey ( IOWrap::waitKey ( 1 ) );
        }

        if ( needKF )
        {
            // 新建一个关键帧
            makeKeyFrame ( fh );
        }
        else
        {
            // 非关键帧
            makeNonKeyFrame ( fh );
        }
    }
    else
    {
        std::unique_lock<std::mutex> lock ( trackMapSyncMutex );
        unmappedTrackedFrames.push_back ( fh );

        if ( needKF )
        {
            needNewKFAfter=fh->shell->trackingRef->id;
        }

        trackedFrameSignal.notify_all();

        while ( coarseTracker_forNewKF->refFrameID == -1 && coarseTracker->refFrameID == -1 )
        {
            mappedFrameSignal.wait ( lock );
        }
    }
}

// 建立一个关键帧
void FullSystem::makeKeyFrame ( FrameHessian* fh )
{
    // needs to be set by mapping thread
    {
        std::unique_lock<std::mutex> crlock ( shellPoseMutex );
        assert ( fh->shell->trackingRef != 0 );
        fh->shell->camToWorld = fh->shell->trackingRef->camToWorld * fh->shell->camToTrackingRef;
        fh->setEvalPT_scaled ( fh->shell->camToWorld.inverse(),fh->shell->aff_g2l );
    }
    traceNewCoarse ( fh );
    std::unique_lock<std::mutex> lock ( mapMutex );
    // =========================== Flag Frames to be Marginalized. =========================
    flagFramesForMarginalization ( fh );

    // =========================== add New Frame to Hessian Struct. =========================
    fh->idx = frameHessians.size();
    frameHessians.push_back ( fh );
    fh->frameID = allKeyFramesHistory.size();
    allKeyFramesHistory.push_back ( fh->shell );
    ef->insertFrame ( fh, &Hcalib );

    setPrecalcValues();

    // =========================== add new residuals for old points =========================
    int numFwdResAdde=0;
    for ( FrameHessian* fh1 : frameHessians )   // go through all active frames
    {
        if ( fh1 == fh )
        {
            continue;
        }
        for ( PointHessian* ph : fh1->pointHessians )
        {
            PointFrameResidual* r = new PointFrameResidual ( ph, fh1, fh );
            r->setState ( ResState::IN );
            ph->residuals.push_back ( r );
            ef->insertResidual ( r );
            ph->lastResiduals[1] = ph->lastResiduals[0];
            ph->lastResiduals[0] = std::pair<PointFrameResidual*, ResState> ( r, ResState::IN );
            numFwdResAdde+=1;
        }
    }
    // =========================== Activate Points (& flag for marginalization). =========================
    activatePointsMT();
    ef->makeIDX();

    // =========================== OPTIMIZE ALL =========================
    fh->frameEnergyTH = frameHessians.back()->frameEnergyTH;
    float rmse = optimize ( setting_maxOptIterations );

    // =========================== Figure Out if INITIALIZATION FAILED =========================
    if ( allKeyFramesHistory.size() <= 4 )
    {
        if ( allKeyFramesHistory.size() ==2 && rmse > 20*benchmark_initializerSlackFactor )
        {
            std::cout <<"I THINK INITIALIZATINO FAILED! Resetting."<<std::endl;
            initFailed=true;
        }
        if ( allKeyFramesHistory.size() ==3 && rmse > 13*benchmark_initializerSlackFactor )
        {
            std::cout<<"I THINK INITIALIZATINO FAILED! Resetting."<<std::endl;
            initFailed=true;
        }
        if ( allKeyFramesHistory.size() ==4 && rmse > 9*benchmark_initializerSlackFactor )
        {
            std::cout <<"I THINK INITIALIZATINO FAILED! Resetting."<<std::endl;
            initFailed=true;
        }
    }
    if ( isLost )
    {
        return;
    }
    // =========================== REMOVE OUTLIER =========================
    removeOutliers();
    {
        std::unique_lock<std::mutex> crlock ( coarseTrackerSwapMutex );
        coarseTracker_forNewKF->makeK ( &Hcalib );
        coarseTracker_forNewKF->setCoarseTrackingRef ( frameHessians );
        coarseTracker_forNewKF->debugPlotIDepthMap ( &minIdJetVisTracker, &maxIdJetVisTracker, outputWrapper );
        coarseTracker_forNewKF->debugPlotIDepthMapFloat ( outputWrapper );
    }
    debugPlot ( "post Optimize" );

    // =========================== (Activate-)Marginalize Points =========================
    flagPointsForRemoval();
    ef->dropPointsF();
    getNullspaces (
        ef->lastNullspaces_pose,
        ef->lastNullspaces_scale,
        ef->lastNullspaces_affA,
        ef->lastNullspaces_affB );
    ef->marginalizePointsF();

    // =========================== add new Immature points & new residuals =========================
    makeNewTraces ( fh, 0 );

    for ( IOWrap::Output3DWrapper* ow : outputWrapper )
    {
        ow->publishGraph ( ef->connectivityMap );
        ow->publishKeyframes ( frameHessians, false, &Hcalib );
    }
    // =========================== Marginalize Frames =========================

    for ( unsigned int i=0; i<frameHessians.size(); i++ )
        if ( frameHessians[i]->flaggedForMarginalization )
        {
            marginalizeFrame ( frameHessians[i] );
            i=0;
        }

    printLogLine();
}

void FullSystem::mappingLoop()
{
    std::unique_lock<std::mutex> lock ( trackMapSyncMutex );

    while ( runMapping )
    {
        while ( unmappedTrackedFrames.size() ==0 )
        {
            trackedFrameSignal.wait ( lock ); // 等待一个新的关键帧
            if ( !runMapping )
            {
                return;
            }
        }

        // 从队列里新取一个关键帧
        FrameHessian* fh = unmappedTrackedFrames.front();
        unmappedTrackedFrames.pop_front();

        // guaranteed to make a KF for the very first two tracked frames.
        if ( allKeyFramesHistory.size() <= 2 )
        {
            lock.unlock();
            makeKeyFrame ( fh );
            lock.lock();
            mappedFrameSignal.notify_all();
            continue;
        }

        if ( unmappedTrackedFrames.size() > 3 )
        {
            // 后台太慢
            needToKetchupMapping=true;
        }

        if ( unmappedTrackedFrames.size() > 0 ) // if there are other frames to tracke, do that first.
        {
            lock.unlock();
            makeNonKeyFrame ( fh );
            lock.lock();

            if ( needToKetchupMapping && unmappedTrackedFrames.size() > 0 )
            {
                // 直接把队列里的删掉
                FrameHessian* fh = unmappedTrackedFrames.front();
                unmappedTrackedFrames.pop_front();
                {
                    std::unique_lock<std::mutex> crlock ( shellPoseMutex );
                    assert ( fh->shell->trackingRef != 0 );
                    fh->shell->camToWorld = fh->shell->trackingRef->camToWorld * fh->shell->camToTrackingRef;
                    fh->setEvalPT_scaled ( fh->shell->camToWorld.inverse(),fh->shell->aff_g2l );
                }
                delete fh;
            }

        }
        else
        {
            if ( setting_realTimeMaxKF || needNewKFAfter >= frameHessians.back()->shell->id )
            {
                lock.unlock();
                makeKeyFrame ( fh );
                needToKetchupMapping=false;
                lock.lock();
            }
            else
            {
                lock.unlock();
                makeNonKeyFrame ( fh );
                lock.lock();
            }
        }
        mappedFrameSignal.notify_all();
    }
    std::cout << "MAPPING FINISHED!" <<std::endl;
}

void FullSystem::makeNonKeyFrame ( FrameHessian* fh )
{
    // needs to be set by mapping thread. no lock required since we are in mapping thread.
    {
        std::unique_lock<std::mutex> crlock ( shellPoseMutex );
        assert ( fh->shell->trackingRef != 0 );
        fh->shell->camToWorld = fh->shell->trackingRef->camToWorld * fh->shell->camToTrackingRef;
        fh->setEvalPT_scaled ( fh->shell->camToWorld.inverse(),fh->shell->aff_g2l );
    }
    traceNewCoarse ( fh );
    delete fh;
}

// 跟踪一个新的帧
Vec4 FullSystem::trackNewCoarse ( FrameHessian* fh )
{
    assert ( allFrameHistory.size() > 0 );
    // set pose initialization. similar with ORB's track with motion model, but tries a lot more possible motions
    // predict current frame's pose using last two frames, adding a random motion

    for ( IOWrap::Output3DWrapper* ow : outputWrapper )
    {
        ow->pushLiveFrame ( fh );
    }

    // 上一个关键帧
    FrameHessian* lastF = coarseTracker->lastRef;

    AffLight aff_last_2_l = AffLight ( 0,0 );

    std::vector<SE3,Eigen::aligned_allocator<SE3>> lastF_2_fh_tries; // 可以尝试的初始值

    // 猜测一些当前帧的Pose
    if ( allFrameHistory.size() == 2 )
        for ( unsigned int i=0; i<lastF_2_fh_tries.size(); i++ )
        {
            lastF_2_fh_tries.push_back ( SE3() );
        }
    else
    {
        FrameShell* slast = allFrameHistory[allFrameHistory.size()-2];
        FrameShell* sprelast = allFrameHistory[allFrameHistory.size()-3];
        SE3 slast_2_sprelast;
        SE3 lastF_2_slast;
        {
            // lock on global pose consistency!
            std::unique_lock<std::mutex> crlock ( shellPoseMutex );
            slast_2_sprelast = sprelast->camToWorld.inverse() * slast->camToWorld;
            lastF_2_slast = slast->camToWorld.inverse() * lastF->shell->camToWorld;
            aff_last_2_l = slast->aff_g2l;
        }
        SE3 fh_2_slast = slast_2_sprelast; // assumed to be the same as fh_2_slast.

        // get last delta-movement.
        lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast ); // assume constant motion.
        lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * fh_2_slast.inverse() * lastF_2_slast ); // assume double motion (frame skipped)
        lastF_2_fh_tries.push_back ( SE3::exp ( fh_2_slast.log() *0.5 ).inverse() * lastF_2_slast ); // assume half motion.
        lastF_2_fh_tries.push_back ( lastF_2_slast ); // assume zero motion.
        lastF_2_fh_tries.push_back ( SE3() ); // assume zero motion FROM KF.

        // just try a TON of different initializations (all rotations). In the end,
        // if they don't work they will only be tried on the coarsest level, which is super fast anyway.
        // also, if tracking rails here we loose, so we really, really want to avoid that.
        // 给预测位姿加上一定扰动作为初始值，作大量的尝试，反正如果误差很大的话，在金字塔前几层就会拒掉，所以很快
        // 基本上是给四元数每个维度加一个扰动
        // 简直了
        for ( float rotDelta=0.02; rotDelta < 0.05; rotDelta++ )
        {
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,rotDelta,0,0 ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,0,rotDelta,0 ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,0,0,rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,-rotDelta,0,0 ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,0,-rotDelta,0 ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,0,0,-rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,rotDelta,rotDelta,0 ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,0,rotDelta,rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,rotDelta,0,rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,-rotDelta,rotDelta,0 ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,0,-rotDelta,rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,-rotDelta,0,rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,rotDelta,-rotDelta,0 ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,0,rotDelta,-rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,rotDelta,0,-rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,-rotDelta,-rotDelta,0 ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,0,-rotDelta,-rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,-rotDelta,0,-rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,-rotDelta,-rotDelta,-rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,-rotDelta,-rotDelta,rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,-rotDelta,rotDelta,-rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,-rotDelta,rotDelta,rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,rotDelta,-rotDelta,-rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,rotDelta,-rotDelta,rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,rotDelta,rotDelta,-rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
            lastF_2_fh_tries.push_back ( fh_2_slast.inverse() * lastF_2_slast * SE3 ( Sophus::Quaterniond ( 1,rotDelta,rotDelta,rotDelta ), Vec3 ( 0,0,0 ) ) ); // assume constant motion.
        }

        if ( !slast->poseValid || !sprelast->poseValid || !lastF->shell->poseValid )
        {
            lastF_2_fh_tries.clear();
            lastF_2_fh_tries.push_back ( SE3() );
        }
    }

    Vec3 flowVecs = Vec3 ( 100,100,100 );
    SE3 lastF_2_fh = SE3();
    AffLight aff_g2l = AffLight ( 0,0 );

    // as long as maxResForImmediateAccept is not reached, I'll continue through the options.
    // I'll keep track of the so-far best achieved residual for each level in achievedRes.
    // If on a coarse level, tracking is WORSE than achievedRes, we will not continue to save time.

    Vec5 achievedRes = Vec5::Constant ( NAN );
    bool haveOneGood = false;
    int tryIterations=0;
    for ( unsigned int i=0; i<lastF_2_fh_tries.size(); i++ )
    {
        // 尝试每一个猜测的位姿
        AffLight aff_g2l_this = aff_last_2_l;
        SE3 lastF_2_fh_this = lastF_2_fh_tries[i];

        bool trackingIsGood = coarseTracker->trackNewestCoarse (
                                  fh, lastF_2_fh_this, aff_g2l_this,
                                  pyrLevelsUsed-1,
                                  achievedRes );  // in each level has to be at least as good as the last try.

        tryIterations++;

        if ( i != 0 )
        {
            printf ( "RE-TRACK ATTEMPT %d with initOption %d and start-lvl %d (ab %f %f): %f %f %f %f %f -> %f %f %f %f %f \n",
                     i,
                     i, pyrLevelsUsed-1,
                     aff_g2l_this.a,aff_g2l_this.b,
                     achievedRes[0],
                     achievedRes[1],
                     achievedRes[2],
                     achievedRes[3],
                     achievedRes[4],
                     coarseTracker->lastResiduals[0],
                     coarseTracker->lastResiduals[1],
                     coarseTracker->lastResiduals[2],
                     coarseTracker->lastResiduals[3],
                     coarseTracker->lastResiduals[4] );
        }

        // do we have a new winner?
        if ( trackingIsGood && std::isfinite ( ( float ) coarseTracker->lastResiduals[0] ) && ! ( coarseTracker->lastResiduals[0] >=  achievedRes[0] ) )
        {
            //printf("take over. minRes %f -> %f!\n", achievedRes[0], coarseTracker->lastResiduals[0]);
            flowVecs = coarseTracker->lastFlowIndicators;
            aff_g2l = aff_g2l_this;
            lastF_2_fh = lastF_2_fh_this;
            haveOneGood = true;
        }

        // take over achieved res (always).
        if ( haveOneGood )
        {
            for ( int i=0; i<5; i++ )
            {
                if ( !std::isfinite ( ( float ) achievedRes[i] ) || achievedRes[i] > coarseTracker->lastResiduals[i] ) // take over if achievedRes is either bigger or NAN.
                {
                    achievedRes[i] = coarseTracker->lastResiduals[i];
                }
            }
        }

        // 只要有一个可行，就接受此结果
        if ( haveOneGood &&  achievedRes[0] < lastCoarseRMSE[0]*setting_reTrackThreshold )
        {
            break;
        }
    }

    if ( !haveOneGood )
    {
        // 一个都没有
        std::cout <<"BIG ERROR! tracking failed entirely. Take predictred pose and hope we may somehow recover."<<std::endl;
        flowVecs = Vec3 ( 0,0,0 );
        aff_g2l = aff_last_2_l;
        lastF_2_fh = lastF_2_fh_tries[0];
    }

    lastCoarseRMSE = achievedRes;

    // no lock required, as fh is not used anywhere yet.
    fh->shell->camToTrackingRef = lastF_2_fh.inverse();
    fh->shell->trackingRef = lastF->shell;
    fh->shell->aff_g2l = aff_g2l;
    fh->shell->camToWorld = fh->shell->trackingRef->camToWorld * fh->shell->camToTrackingRef;

    if ( coarseTracker->firstCoarseRMSE < 0 )
    {
        coarseTracker->firstCoarseRMSE = achievedRes[0];
    }

    if ( !setting_debugout_runquiet )
    {
        printf ( "Coarse Tracker tracked ab = %f %f (exp %f). Res %f!\n", aff_g2l.a, aff_g2l.b, fh->ab_exposure, achievedRes[0] );
    }

    if ( setting_logStuff )
    {
        ( *coarseTrackingLog ) << std::setprecision ( 16 )
                               << fh->shell->id << " "
                               << fh->shell->timestamp << " "
                               << fh->ab_exposure << " "
                               << fh->shell->camToWorld.log().transpose() << " "
                               << aff_g2l.a << " "
                               << aff_g2l.b << " "
                               << achievedRes[0] << " "
                               << tryIterations << "\n";
    }

    return Vec4 ( achievedRes[0], flowVecs[0], flowVecs[1], flowVecs[2] );

}

void FullSystem::traceNewCoarse ( FrameHessian* fh )
{
    std::unique_lock<mutex> lock ( mapMutex );
    int trace_total=0, trace_good=0, trace_oob=0, trace_out=0, trace_skip=0, trace_badcondition=0, trace_uninitialized=0;

    Mat33f K = Mat33f::Identity();
    K ( 0,0 ) = Hcalib.fxl();
    K ( 1,1 ) = Hcalib.fyl();
    K ( 0,2 ) = Hcalib.cxl();
    K ( 1,2 ) = Hcalib.cyl();

    for ( FrameHessian* host : frameHessians )  // go through all active frames
    {
        SE3 hostToNew = fh->PRE_worldToCam * host->PRE_camToWorld;
        Mat33f KRKi = K * hostToNew.rotationMatrix().cast<float>() * K.inverse();
        Vec3f Kt = K * hostToNew.translation().cast<float>();

        Vec2f aff = AffLight::fromToVecExposure ( host->ab_exposure, fh->ab_exposure, host->aff_g2l(), fh->aff_g2l() ).cast<float>();

        // 更新关键帧中未成熟的地图点
        for ( ImmaturePoint* ph : host->immaturePoints )
        {
            ph->traceOn ( fh, KRKi, Kt, aff, &Hcalib, false );

            if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_GOOD )
            {
                trace_good++;
            }
            if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_BADCONDITION )
            {
                trace_badcondition++;
            }
            if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_OOB )
            {
                trace_oob++;
            }
            if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_OUTLIER )
            {
                trace_out++;
            }
            if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_SKIPPED )
            {
                trace_skip++;
            }
            if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_UNINITIALIZED )
            {
                trace_uninitialized++;
            }
            trace_total++;
        }
    }
}

float FullSystem::optimize ( int mnumOptIts )
{
    if ( frameHessians.size() < 2 )
    {
        return 0;
    }
    if ( frameHessians.size() < 3 )
    {
        mnumOptIts = 20;
    }
    if ( frameHessians.size() < 4 )
    {
        mnumOptIts = 15;
    }

    // get statistics and active residuals.
    activeResiduals.clear();
    int numPoints = 0;
    int numLRes = 0;
    for ( FrameHessian* fh : frameHessians )
        for ( PointHessian* ph : fh->pointHessians )
        {
            for ( PointFrameResidual* r : ph->residuals )
            {
                if ( !r->efResidual->isLinearized )
                {
                    activeResiduals.push_back ( r );
                    r->resetOOB();
                }
                else
                {
                    numLRes++;
                }
            }
            numPoints++;
        }

    if ( !setting_debugout_runquiet )
    {
        printf ( "OPTIMIZE %d pts, %d active res, %d lin res!\n",ef->nPoints, ( int ) activeResiduals.size(), numLRes );
    }

    Vec3 lastEnergy = linearizeAll ( false );
    double lastEnergyL = calcLEnergy();
    double lastEnergyM = calcMEnergy();

    if ( multiThreading )
    {
        treadReduce.reduce ( bind ( &FullSystem::applyRes_Reductor, this, true, placeholders::_1, placeholders::_2, placeholders::_3, placeholders::_4 ), 0, activeResiduals.size(), 50 );
    }
    else
    {
        applyRes_Reductor ( true,0,activeResiduals.size(),0,0 );
    }

    if ( !setting_debugout_runquiet )
    {
        printf ( "Initial Error       \t" );
        printOptRes ( lastEnergy, lastEnergyL, lastEnergyM, 0, 0, frameHessians.back()->aff_g2l().a, frameHessians.back()->aff_g2l().b );
    }

    debugPlotTracking();

    double lambda = 1e-1;
    float stepsize=1;
    VecX previousX = VecX::Constant ( CPARS+ 8*frameHessians.size(), NAN );
    for ( int iteration=0; iteration<mnumOptIts; iteration++ )
    {
        // solve!
        backupState ( iteration!=0 );
        solveSystem ( iteration, lambda );
        double incDirChange = ( 1e-20 + previousX.dot ( ef->lastX ) ) / ( 1e-20 + previousX.norm() * ef->lastX.norm() );
        previousX = ef->lastX;

        if ( std::isfinite ( incDirChange ) && ( setting_solverMode & SOLVER_STEPMOMENTUM ) )
        {
            float newStepsize = exp ( incDirChange*1.4 );
            if ( incDirChange<0 && stepsize>1 )
            {
                stepsize=1;
            }

            stepsize = sqrtf ( sqrtf ( newStepsize*stepsize*stepsize*stepsize ) );
            if ( stepsize > 2 )
            {
                stepsize=2;
            }
            if ( stepsize <0.25 )
            {
                stepsize=0.25;
            }
        }

        bool canbreak = doStepFromBackup ( stepsize,stepsize,stepsize,stepsize,stepsize );

        // eval new energy!
        Vec3 newEnergy = linearizeAll ( false );
        double newEnergyL = calcLEnergy();
        double newEnergyM = calcMEnergy();

        if ( !setting_debugout_runquiet )
        {
            printf ( "%s %d (L %.2f, dir %.2f, ss %.1f): \t",
                     ( newEnergy[0] +  newEnergy[1] +  newEnergyL + newEnergyM <
                       lastEnergy[0] + lastEnergy[1] + lastEnergyL + lastEnergyM ) ? "ACCEPT" : "REJECT",
                     iteration,
                     log10 ( lambda ),
                     incDirChange,
                     stepsize );
            printOptRes ( newEnergy, newEnergyL, newEnergyM , 0, 0, frameHessians.back()->aff_g2l().a, frameHessians.back()->aff_g2l().b );
        }

        if ( setting_forceAceptStep || ( newEnergy[0] +  newEnergy[1] +  newEnergyL + newEnergyM <
                                         lastEnergy[0] + lastEnergy[1] + lastEnergyL + lastEnergyM ) )
        {
            if ( multiThreading )
            {
                treadReduce.reduce ( bind ( &FullSystem::applyRes_Reductor, this, true, _1, _2, _3, _4 ), 0, activeResiduals.size(), 50 );
            }
            else
            {
                applyRes_Reductor ( true,0,activeResiduals.size(),0,0 );
            }

            lastEnergy = newEnergy;
            lastEnergyL = newEnergyL;
            lastEnergyM = newEnergyM;

            lambda *= 0.25;
        }
        else
        {
            loadSateBackup();
            lastEnergy = linearizeAll ( false );
            lastEnergyL = calcLEnergy();
            lastEnergyM = calcMEnergy();
            lambda *= 1e2;
        }

        if ( canbreak && iteration >= setting_minOptIterations )
        {
            break;
        }
    }

    Vec10 newStateZero = Vec10::Zero();
    newStateZero.segment<2> ( 6 ) = frameHessians.back()->get_state().segment<2> ( 6 );

    frameHessians.back()->setEvalPT ( frameHessians.back()->PRE_worldToCam,
                                      newStateZero );
    EFDeltaValid=false;
    EFAdjointsValid=false;
    ef->setAdjointsF ( &Hcalib );
    setPrecalcValues();

    lastEnergy = linearizeAll ( true );

    if ( !std::isfinite ( ( double ) lastEnergy[0] ) || !std::isfinite ( ( double ) lastEnergy[1] ) || !std::isfinite ( ( double ) lastEnergy[2] ) )
    {
        printf ( "KF Tracking failed: LOST!\n" );
        isLost=true;
    }
    statistics_lastFineTrackRMSE = sqrtf ( ( float ) ( lastEnergy[0] / ( patternNum*ef->resInA ) ) );

    if ( calibLog != 0 )
    {
        ( *calibLog ) << Hcalib.value_scaled.transpose() <<
                      " " << frameHessians.back()->get_state_scaled().transpose() <<
                      " " << sqrtf ( ( float ) ( lastEnergy[0] / ( patternNum*ef->resInA ) ) ) <<
                      " " << ef->resInM << "\n";
        calibLog->flush();
    }

    {
        unique_lock<mutex> crlock ( shellPoseMutex );
        for ( FrameHessian* fh : frameHessians )
        {
            fh->shell->camToWorld = fh->PRE_camToWorld;
            fh->shell->aff_g2l = fh->aff_g2l();
        }
    }
    debugPlotTracking();
    return sqrtf ( ( float ) ( lastEnergy[0] / ( patternNum*ef->resInA ) ) );
}

void FullSystem::solveSystem ( int iteration, double lambda )
{
    ef->lastNullspaces_forLogging = getNullspaces (
                                        ef->lastNullspaces_pose,
                                        ef->lastNullspaces_scale,
                                        ef->lastNullspaces_affA,
                                        ef->lastNullspaces_affB );

    ef->solveSystemF ( iteration, lambda,&Hcalib );
}

void FullSystem::activatePointsMT_Reductor (
    std::vector<PointHessian*>* optimized,std::vector<ImmaturePoint*>* toOptimize,int min, int max, Vec10* stats, int tid )
{
    ImmaturePointTemporaryResidual* tr = new ImmaturePointTemporaryResidual[frameHessians.size()];
    for ( int k=min; k<max; k++ )
    {
        ( *optimized ) [k] = optimizeImmaturePoint ( ( *toOptimize ) [k],1,tr );
    }
    delete[] tr;
}

void FullSystem::activatePointsMT()
{
    if ( ef->nPoints < setting_desiredPointDensity*0.66 )
    {
        currentMinActDist -= 0.8;
    }
    if ( ef->nPoints < setting_desiredPointDensity*0.8 )
    {
        currentMinActDist -= 0.5;
    }
    else if ( ef->nPoints < setting_desiredPointDensity*0.9 )
    {
        currentMinActDist -= 0.2;
    }
    else if ( ef->nPoints < setting_desiredPointDensity )
    {
        currentMinActDist -= 0.1;
    }

    if ( ef->nPoints > setting_desiredPointDensity*1.5 )
    {
        currentMinActDist += 0.8;
    }
    if ( ef->nPoints > setting_desiredPointDensity*1.3 )
    {
        currentMinActDist += 0.5;
    }
    if ( ef->nPoints > setting_desiredPointDensity*1.15 )
    {
        currentMinActDist += 0.2;
    }
    if ( ef->nPoints > setting_desiredPointDensity )
    {
        currentMinActDist += 0.1;
    }

    if ( currentMinActDist < 0 )
    {
        currentMinActDist = 0;
    }
    if ( currentMinActDist > 4 )
    {
        currentMinActDist = 4;
    }

    if ( !setting_debugout_runquiet )
        printf ( "SPARSITY:  MinActDist %f (need %d points, have %d points)!\n",
                 currentMinActDist, ( int ) ( setting_desiredPointDensity ), ef->nPoints );

    FrameHessian* newestHs = frameHessians.back();

    // make dist map.
    coarseDistanceMap->makeK ( &Hcalib );
    coarseDistanceMap->makeDistanceMap ( frameHessians, newestHs );

    std::vector<ImmaturePoint*> toOptimize;
    toOptimize.reserve ( 20000 );

    for ( FrameHessian* host : frameHessians )      // go through all active frames
    {
        if ( host == newestHs )
        {
            continue;
        }

        SE3 fhToNew = newestHs->PRE_worldToCam * host->PRE_camToWorld;
        Mat33f KRKi = ( coarseDistanceMap->K[1] * fhToNew.rotationMatrix().cast<float>() * coarseDistanceMap->Ki[0] );
        Vec3f Kt = ( coarseDistanceMap->K[1] * fhToNew.translation().cast<float>() );

        for ( unsigned int i=0; i<host->immaturePoints.size(); i+=1 )
        {
            ImmaturePoint* ph = host->immaturePoints[i];
            ph->idxInImmaturePoints = i;

            // delete points that have never been traced successfully, or that are outlier on the last trace.
            if ( !std::isfinite ( ph->idepth_max ) || ph->lastTraceStatus == IPS_OUTLIER )
            {
                // remove point.
                delete ph;
                host->immaturePoints[i]=0;
                continue;
            }

            // can activate only if this is true.
            bool canActivate = ( ph->lastTraceStatus == IPS_GOOD
                                 || ph->lastTraceStatus == IPS_SKIPPED
                                 || ph->lastTraceStatus == IPS_BADCONDITION
                                 || ph->lastTraceStatus == IPS_OOB )
                               && ph->lastTracePixelInterval < 8
                               && ph->quality > setting_minTraceQuality
                               && ( ph->idepth_max+ph->idepth_min ) > 0;


            // if I cannot activate the point, skip it. Maybe also delete it.
            if ( !canActivate )
            {
                // if point will be out afterwards, delete it instead.
                if ( ph->host->flaggedForMarginalization || ph->lastTraceStatus == IPS_OOB )
                {
                    delete ph;
                    host->immaturePoints[i]=0;
                }
                continue;
            }

            // see if we need to activate point due to distance map.
            Vec3f ptp = KRKi * Vec3f ( ph->u, ph->v, 1 ) + Kt* ( 0.5f* ( ph->idepth_max+ph->idepth_min ) );
            int u = ptp[0] / ptp[2] + 0.5f;
            int v = ptp[1] / ptp[2] + 0.5f;

            if ( ( u > 0 && v > 0 && u < wG[1] && v < hG[1] ) )
            {

                float dist = coarseDistanceMap->fwdWarpedIDDistFinal[u+wG[1]*v] + ( ptp[0]-floorf ( ( float ) ( ptp[0] ) ) );

                if ( dist>=currentMinActDist* ph->my_type )
                {
                    coarseDistanceMap->addIntoDistFinal ( u,v );
                    toOptimize.push_back ( ph );
                }
            }
            else
            {
                delete ph;
                host->immaturePoints[i]=0;
            }
        }
    }

    std::vector<PointHessian*> optimized;
    optimized.resize ( toOptimize.size() );

    if ( multiThreading )
    {
        treadReduce.reduce ( bind ( &FullSystem::activatePointsMT_Reductor, this, &optimized, &toOptimize, _1, _2, _3, _4 ), 0, toOptimize.size(), 50 );
    }
    else
    {
        activatePointsMT_Reductor ( &optimized, &toOptimize, 0, toOptimize.size(), 0, 0 );
    }


    for ( unsigned k=0; k<toOptimize.size(); k++ )
    {
        PointHessian* newpoint = optimized[k];
        ImmaturePoint* ph = toOptimize[k];

        if ( newpoint != 0 && newpoint != ( PointHessian* ) ( ( long ) ( -1 ) ) )
        {
            newpoint->host->immaturePoints[ph->idxInImmaturePoints]=0;
            newpoint->host->pointHessians.push_back ( newpoint );
            ef->insertPoint ( newpoint );
            for ( PointFrameResidual* r : newpoint->residuals )
            {
                ef->insertResidual ( r );
            }
            assert ( newpoint->efPoint != 0 );
            delete ph;
        }
        else if ( newpoint == ( PointHessian* ) ( ( long ) ( -1 ) ) || ph->lastTraceStatus==IPS_OOB )
        {
            delete ph;
            ph->host->immaturePoints[ph->idxInImmaturePoints]=0;
        }
        else
        {
            assert ( newpoint == 0 || newpoint == ( PointHessian* ) ( ( long ) ( -1 ) ) );
        }
    }

    for ( FrameHessian* host : frameHessians )
    {
        for ( int i=0; i< ( int ) host->immaturePoints.size(); i++ )
        {
            if ( host->immaturePoints[i]==0 )
            {
                host->immaturePoints[i] = host->immaturePoints.back();
                host->immaturePoints.pop_back();
                i--;
            }
        }
    }

}

PointHessian* FullSystem::optimizeImmaturePoint ( ImmaturePoint* point, int minObs, ImmaturePointTemporaryResidual* residuals )
{
    // 优化单个未成熟地图点
    int nres = 0;
    for ( FrameHessian* fh : frameHessians )
    {
        if ( fh != point->host )
        {
            residuals[nres].state_NewEnergy = residuals[nres].state_energy = 0;
            residuals[nres].state_NewState = ResState::OUTLIER;
            residuals[nres].state_state = ResState::IN;
            residuals[nres].target = fh;
            nres++;
        }
    }
    assert ( nres == ( ( int ) frameHessians.size() )-1 );

    bool print = false;//rand()%50==0;

    float lastEnergy = 0;
    float lastHdd=0;
    float lastbd=0;
    float currentIdepth= ( point->idepth_max+point->idepth_min ) *0.5f;

    for ( int i=0; i<nres; i++ )
    {
        lastEnergy += point->linearizeResidual ( &Hcalib, 1000, residuals+i,lastHdd, lastbd, currentIdepth );
        residuals[i].state_state = residuals[i].state_NewState;
        residuals[i].state_energy = residuals[i].state_NewEnergy;
    }

    if ( !std::isfinite ( lastEnergy ) || lastHdd < setting_minIdepthH_act )
    {
        if ( print )
            printf ( "OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n",
                     nres, lastHdd, lastEnergy );
        return 0;
    }

    if ( print ) printf ( "Activate point. %d residuals. H=%f. Initial Energy: %f. Initial Id=%f\n" ,
                              nres, lastHdd,lastEnergy,currentIdepth );

    // do L-M iterations to update the idepth
    float lambda = 0.1;
    for ( int iteration=0; iteration<setting_GNItsOnPointActivation; iteration++ )
    {
        float H = lastHdd;
        H *= 1+lambda;
        float step = ( 1.0/H ) * lastbd;
        float newIdepth = currentIdepth - step;

        float newHdd=0;
        float newbd=0;
        float newEnergy=0;
        for ( int i=0; i<nres; i++ )
        {
            newEnergy += point->linearizeResidual ( &Hcalib, 1, residuals+i,newHdd, newbd, newIdepth );
        }

        if ( !std::isfinite ( lastEnergy ) || newHdd < setting_minIdepthH_act )
        {
            if ( print ) printf ( "OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n",
                                      nres,
                                      newHdd,
                                      lastEnergy );
            return 0;
        }

        if ( print ) printf ( "%s %d (L %.2f) %s: %f -> %f (idepth %f)!\n",
                                  ( true || newEnergy < lastEnergy ) ? "ACCEPT" : "REJECT",
                                  iteration,
                                  log10 ( lambda ),
                                  "",
                                  lastEnergy, newEnergy, newIdepth );

        if ( newEnergy < lastEnergy )
        {
            currentIdepth = newIdepth;
            lastHdd = newHdd;
            lastbd = newbd;
            lastEnergy = newEnergy;
            for ( int i=0; i<nres; i++ )
            {
                residuals[i].state_state = residuals[i].state_NewState;
                residuals[i].state_energy = residuals[i].state_NewEnergy;
            }

            lambda *= 0.5;
        }
        else
        {
            lambda *= 5;
        }

        if ( fabsf ( step ) < 0.0001*currentIdepth )
        {
            break;
        }
    }

    if ( !std::isfinite ( currentIdepth ) )
    {
        printf ( "MAJOR ERROR! point idepth is nan after initialization (%f).\n", currentIdepth );
        // -1的指针。。。。
        return ( PointHessian* ) ( ( long ) ( -1 ) );   // yeah I'm like 99% sure this is OK on 32bit systems.
    }


    int numGoodRes=0;
    for ( int i=0; i<nres; i++ )
        if ( residuals[i].state_state == ResState::IN )
        {
            numGoodRes++;
        }

    if ( numGoodRes < minObs )
    {
        if ( print )
        {
            printf ( "OptPoint: OUTLIER!\n" );
        }
        // -1的指针。。。。
        return ( PointHessian* ) ( ( long ) ( -1 ) );   // yeah I'm like 99% sure this is OK on 32bit systems.
    }

    PointHessian* p = new PointHessian ( point, &Hcalib );
    if ( !std::isfinite ( p->energyTH ) )
    {
        delete p;
        return ( PointHessian* ) ( ( long ) ( -1 ) );
    }

    p->lastResiduals[0].first = 0;
    p->lastResiduals[0].second = ResState::OOB;
    p->lastResiduals[1].first = 0;
    p->lastResiduals[1].second = ResState::OOB;
    p->setIdepthZero ( currentIdepth );
    p->setIdepth ( currentIdepth );
    p->setPointStatus ( PointHessian::ACTIVE );

    for ( int i=0; i<nres; i++ )
        if ( residuals[i].state_state == ResState::IN )
        {
            PointFrameResidual* r = new PointFrameResidual ( p, p->host, residuals[i].target );
            r->state_NewEnergy = r->state_energy = 0;
            r->state_NewState = ResState::OUTLIER;
            r->setState ( ResState::IN );
            p->residuals.push_back ( r );

            if ( r->target == frameHessians.back() )
            {
                p->lastResiduals[0].first = r;
                p->lastResiduals[0].second = ResState::IN;
            }
            else if ( r->target == ( frameHessians.size() <2 ? 0 : frameHessians[frameHessians.size()-2] ) )
            {
                p->lastResiduals[1].first = r;
                p->lastResiduals[1].second = ResState::IN;
            }
        }

    if ( print )
    {
        printf ( "point activated!\n" );
    }

    statistics_numActivatedPoints++;
    return p;
}

void FullSystem::initializeFromInitializer ( FrameHessian* newFrame )
{
    unique_lock<mutex> lock ( mapMutex );

    // add firstframe.
    FrameHessian* firstFrame = coarseInitializer->firstFrame;
    firstFrame->idx = frameHessians.size();
    frameHessians.push_back ( firstFrame );
    firstFrame->frameID = allKeyFramesHistory.size();
    allKeyFramesHistory.push_back ( firstFrame->shell );
    ef->insertFrame ( firstFrame, &Hcalib );
    setPrecalcValues();
    firstFrame->pointHessians.reserve ( wG[0]*hG[0]*0.2f );
    firstFrame->pointHessiansMarginalized.reserve ( wG[0]*hG[0]*0.2f );
    firstFrame->pointHessiansOut.reserve ( wG[0]*hG[0]*0.2f );
    float sumID=1e-5, numID=1e-5;
    for ( int i=0; i<coarseInitializer->numPoints[0]; i++ )
    {
        sumID += coarseInitializer->points[0][i].iR;
        numID++;
    }
    float rescaleFactor = 1 / ( sumID / numID );

    // randomly sub-select the points I need.
    float keepPercentage = setting_desiredPointDensity / coarseInitializer->numPoints[0];

    if ( !setting_debugout_runquiet )
        printf ( "Initialization: keep %.1f%% (need %d, have %d)!\n", 100*keepPercentage,
                 ( int ) ( setting_desiredPointDensity ), coarseInitializer->numPoints[0] );

    for ( int i=0; i<coarseInitializer->numPoints[0]; i++ )
    {
        if ( rand() / ( float ) RAND_MAX > keepPercentage )
        {
            continue;
        }

        Pnt* point = coarseInitializer->points[0]+i;
        ImmaturePoint* pt = new ImmaturePoint ( point->u+0.5f,point->v+0.5f,firstFrame,point->my_type, &Hcalib );

        if ( !std::isfinite ( pt->energyTH ) )
        {
            delete pt;
            continue;
        }


        pt->idepth_max=pt->idepth_min=1;
        PointHessian* ph = new PointHessian ( pt, &Hcalib );
        delete pt;
        if ( !std::isfinite ( ph->energyTH ) )
        {
            delete ph;
            continue;
        }

        ph->setIdepthScaled ( point->iR*rescaleFactor );
        ph->setIdepthZero ( ph->idepth );
        ph->hasDepthPrior=true;
        ph->setPointStatus ( PointHessian::ACTIVE );

        firstFrame->pointHessians.push_back ( ph );
        ef->insertPoint ( ph );
    }

    SE3 firstToNew = coarseInitializer->thisToNext;
    firstToNew.translation() /= rescaleFactor;
    // really no lock required, as we are initializing.
    {
        unique_lock<mutex> crlock ( shellPoseMutex );
        firstFrame->shell->camToWorld = SE3();
        firstFrame->shell->aff_g2l = AffLight ( 0,0 );
        firstFrame->setEvalPT_scaled ( firstFrame->shell->camToWorld.inverse(),firstFrame->shell->aff_g2l );
        firstFrame->shell->trackingRef=0;
        firstFrame->shell->camToTrackingRef = SE3();

        newFrame->shell->camToWorld = firstToNew.inverse();
        newFrame->shell->aff_g2l = AffLight ( 0,0 );
        newFrame->setEvalPT_scaled ( newFrame->shell->camToWorld.inverse(),newFrame->shell->aff_g2l );
        newFrame->shell->trackingRef = firstFrame->shell;
        newFrame->shell->camToTrackingRef = firstToNew.inverse();

    }

    initialized=true;
    printf ( "INITIALIZE FROM INITIALIZER (%d pts)!\n", ( int ) firstFrame->pointHessians.size() );
}

void FullSystem::makeNewTraces ( FrameHessian* newFrame, float* gtDepth )
{
    pixelSelector->allowFast = true;
    //int numPointsTotal = makePixelStatus(newFrame->dI, selectionMap, wG[0], hG[0], setting_desiredDensity);
    int numPointsTotal = pixelSelector->makeMaps ( newFrame, selectionMap,setting_desiredImmatureDensity );

    newFrame->pointHessians.reserve ( numPointsTotal*1.2f );
    //fh->pointHessiansInactive.reserve(numPointsTotal*1.2f);
    newFrame->pointHessiansMarginalized.reserve ( numPointsTotal*1.2f );
    newFrame->pointHessiansOut.reserve ( numPointsTotal*1.2f );

    for ( int y=patternPadding+1; y<hG[0]-patternPadding-2; y++ )
        for ( int x=patternPadding+1; x<wG[0]-patternPadding-2; x++ )
        {
            int i = x+y*wG[0];
            if ( selectionMap[i]==0 )
            {
                continue;
            }

            ImmaturePoint* impt = new ImmaturePoint ( x,y,newFrame, selectionMap[i], &Hcalib );
            if ( !std::isfinite ( impt->energyTH ) )
            {
                delete impt;
            }
            else
            {
                newFrame->immaturePoints.push_back ( impt );
            }

        }
}

void FullSystem::linearizeAll_Reductor ( bool fixLinearization, std::vector<PointFrameResidual*>* toRemove, int min, int max, Vec10* stats, int tid )
{
    for ( int k=min; k<max; k++ )
    {
        PointFrameResidual* r = activeResiduals[k];
        ( *stats ) [0] += r->linearize ( &Hcalib );

        if ( fixLinearization )
        {
            r->applyRes ( true );

            if ( r->efResidual->isActive() )
            {
                if ( r->isNew )
                {
                    PointHessian* p = r->point;
                    Vec3f ptp_inf = r->host->targetPrecalc[r->target->idx].PRE_KRKiTll * Vec3f ( p->u,p->v, 1 );    // projected point assuming infinite depth.
                    Vec3f ptp = ptp_inf + r->host->targetPrecalc[r->target->idx].PRE_KtTll*p->idepth_scaled;        // projected point with real depth.
                    float relBS = 0.01* ( ( ptp_inf.head<2>() / ptp_inf[2] )- ( ptp.head<2>() / ptp[2] ) ).norm(); // 0.01 = one pixel.


                    if ( relBS > p->maxRelBaseline )
                    {
                        p->maxRelBaseline = relBS;
                    }

                    p->numGoodResiduals++;
                }
            }
            else
            {
                toRemove[tid].push_back ( activeResiduals[k] );
            }
        }
    }
}

void FullSystem::applyRes_Reductor ( bool copyJacobians, int min, int max, Vec10* stats, int tid )
{
    for ( int k=min; k<max; k++ )
    {
        activeResiduals[k]->applyRes ( true );
    }
}

void FullSystem::setNewFrameEnergyTH()
{
    // collect all residuals and make decision on TH.
    allResVec.clear();
    allResVec.reserve ( activeResiduals.size() *2 );
    FrameHessian* newFrame = frameHessians.back();

    for ( PointFrameResidual* r : activeResiduals )
        if ( r->state_NewEnergyWithOutlier >= 0 && r->target == newFrame )
        {
            allResVec.push_back ( r->state_NewEnergyWithOutlier );

        }

    if ( allResVec.size() ==0 )
    {
        newFrame->frameEnergyTH = 12*12*patternNum;
        return;         // should never happen, but lets make sure.
    }

    int nthIdx = setting_frameEnergyTHN*allResVec.size();

    assert ( nthIdx < ( int ) allResVec.size() );
    assert ( setting_frameEnergyTHN < 1 );

    std::nth_element ( allResVec.begin(), allResVec.begin() +nthIdx, allResVec.end() );
    float nthElement = sqrtf ( allResVec[nthIdx] );
    newFrame->frameEnergyTH = nthElement*setting_frameEnergyTHFacMedian;
    newFrame->frameEnergyTH = 26.0f*setting_frameEnergyTHConstWeight + newFrame->frameEnergyTH* ( 1-setting_frameEnergyTHConstWeight );
    newFrame->frameEnergyTH = newFrame->frameEnergyTH*newFrame->frameEnergyTH;
    newFrame->frameEnergyTH *= setting_overallEnergyTHWeight*setting_overallEnergyTHWeight;
}

Vec3 FullSystem::linearizeAll ( bool fixLinearization )
{
    double lastEnergyP = 0;
    double lastEnergyR = 0;
    double num = 0;


    std::vector<PointFrameResidual*> toRemove[NUM_THREADS];
    for ( int i=0; i<NUM_THREADS; i++ )
    {
        toRemove[i].clear();
    }

    if ( multiThreading )
    {
        treadReduce.reduce ( bind ( &FullSystem::linearizeAll_Reductor, this, fixLinearization, toRemove, _1, _2, _3, _4 ), 0, activeResiduals.size(), 0 );
        lastEnergyP = treadReduce.stats[0];
    }
    else
    {
        Vec10 stats;
        linearizeAll_Reductor ( fixLinearization, toRemove, 0,activeResiduals.size(),&stats,0 );
        lastEnergyP = stats[0];
    }


    setNewFrameEnergyTH();


    if ( fixLinearization )
    {

        for ( PointFrameResidual* r : activeResiduals )
        {
            PointHessian* ph = r->point;
            if ( ph->lastResiduals[0].first == r )
            {
                ph->lastResiduals[0].second = r->state_state;
            }
            else if ( ph->lastResiduals[1].first == r )
            {
                ph->lastResiduals[1].second = r->state_state;
            }



        }

        int nResRemoved=0;
        for ( int i=0; i<NUM_THREADS; i++ )
        {
            for ( PointFrameResidual* r : toRemove[i] )
            {
                PointHessian* ph = r->point;

                if ( ph->lastResiduals[0].first == r )
                {
                    ph->lastResiduals[0].first=0;
                }
                else if ( ph->lastResiduals[1].first == r )
                {
                    ph->lastResiduals[1].first=0;
                }

                for ( unsigned int k=0; k<ph->residuals.size(); k++ )
                    if ( ph->residuals[k] == r )
                    {
                        ef->dropResidual ( r->efResidual );
                        deleteOut<PointFrameResidual> ( ph->residuals,k );
                        nResRemoved++;
                        break;
                    }
            }
        }
        //printf("FINAL LINEARIZATION: removed %d / %d residuals!\n", nResRemoved, (int)activeResiduals.size());

    }

    return Vec3 ( lastEnergyP, lastEnergyR, num );
}

// applies step to linearization point.
bool FullSystem::doStepFromBackup ( float stepfacC,float stepfacT,float stepfacR,float stepfacA,float stepfacD )
{
    Vec10 pstepfac;
    pstepfac.segment<3> ( 0 ).setConstant ( stepfacT );
    pstepfac.segment<3> ( 3 ).setConstant ( stepfacR );
    pstepfac.segment<4> ( 6 ).setConstant ( stepfacA );


    float sumA=0, sumB=0, sumT=0, sumR=0, sumID=0, numID=0;

    float sumNID=0;

    if ( setting_solverMode & SOLVER_MOMENTUM )
    {
        Hcalib.setValue ( Hcalib.value_backup + Hcalib.step );
        for ( FrameHessian* fh : frameHessians )
        {
            Vec10 step = fh->step;
            step.head<6>() += 0.5f* ( fh->step_backup.head<6>() );

            fh->setState ( fh->state_backup + step );
            sumA += step[6]*step[6];
            sumB += step[7]*step[7];
            sumT += step.segment<3> ( 0 ).squaredNorm();
            sumR += step.segment<3> ( 3 ).squaredNorm();

            for ( PointHessian* ph : fh->pointHessians )
            {
                float step = ph->step+0.5f* ( ph->step_backup );
                ph->setIdepth ( ph->idepth_backup + step );
                sumID += step*step;
                sumNID += fabsf ( ph->idepth_backup );
                numID++;

                ph->setIdepthZero ( ph->idepth_backup + step );
            }
        }
    }
    else
    {
        Hcalib.setValue ( Hcalib.value_backup + stepfacC*Hcalib.step );
        for ( FrameHessian* fh : frameHessians )
        {
            fh->setState ( fh->state_backup + pstepfac.cwiseProduct ( fh->step ) );
            sumA += fh->step[6]*fh->step[6];
            sumB += fh->step[7]*fh->step[7];
            sumT += fh->step.segment<3> ( 0 ).squaredNorm();
            sumR += fh->step.segment<3> ( 3 ).squaredNorm();

            for ( PointHessian* ph : fh->pointHessians )
            {
                ph->setIdepth ( ph->idepth_backup + stepfacD*ph->step );
                sumID += ph->step*ph->step;
                sumNID += fabsf ( ph->idepth_backup );
                numID++;

                ph->setIdepthZero ( ph->idepth_backup + stepfacD*ph->step );
            }
        }
    }

    sumA /= frameHessians.size();
    sumB /= frameHessians.size();
    sumR /= frameHessians.size();
    sumT /= frameHessians.size();
    sumID /= numID;
    sumNID /= numID;

    if ( !setting_debugout_runquiet )
        printf ( "STEPS: A %.1f; B %.1f; R %.1f; T %.1f. \t",
                 sqrtf ( sumA ) / ( 0.0005*setting_thOptIterations ),
                 sqrtf ( sumB ) / ( 0.00005*setting_thOptIterations ),
                 sqrtf ( sumR ) / ( 0.00005*setting_thOptIterations ),
                 sqrtf ( sumT ) *sumNID / ( 0.00005*setting_thOptIterations ) );


    EFDeltaValid=false;
    setPrecalcValues();

    return sqrtf ( sumA ) < 0.0005*setting_thOptIterations &&
           sqrtf ( sumB ) < 0.00005*setting_thOptIterations &&
           sqrtf ( sumR ) < 0.00005*setting_thOptIterations &&
           sqrtf ( sumT ) *sumNID < 0.00005*setting_thOptIterations;
}



// sets linearization point.
void FullSystem::backupState ( bool backupLastStep )
{
    if ( setting_solverMode & SOLVER_MOMENTUM )
    {
        if ( backupLastStep )
        {
            Hcalib.step_backup = Hcalib.step;
            Hcalib.value_backup = Hcalib.value;
            for ( FrameHessian* fh : frameHessians )
            {
                fh->step_backup = fh->step;
                fh->state_backup = fh->get_state();
                for ( PointHessian* ph : fh->pointHessians )
                {
                    ph->idepth_backup = ph->idepth;
                    ph->step_backup = ph->step;
                }
            }
        }
        else
        {
            Hcalib.step_backup.setZero();
            Hcalib.value_backup = Hcalib.value;
            for ( FrameHessian* fh : frameHessians )
            {
                fh->step_backup.setZero();
                fh->state_backup = fh->get_state();
                for ( PointHessian* ph : fh->pointHessians )
                {
                    ph->idepth_backup = ph->idepth;
                    ph->step_backup=0;
                }
            }
        }
    }
    else
    {
        Hcalib.value_backup = Hcalib.value;
        for ( FrameHessian* fh : frameHessians )
        {
            fh->state_backup = fh->get_state();
            for ( PointHessian* ph : fh->pointHessians )
            {
                ph->idepth_backup = ph->idepth;
            }
        }
    }
}

// sets linearization point.
void FullSystem::loadSateBackup()
{
    Hcalib.setValue ( Hcalib.value_backup );
    for ( FrameHessian* fh : frameHessians )
    {
        fh->setState ( fh->state_backup );
        for ( PointHessian* ph : fh->pointHessians )
        {
            ph->setIdepth ( ph->idepth_backup );

            ph->setIdepthZero ( ph->idepth_backup );
        }

    }

    EFDeltaValid=false;
    setPrecalcValues();
}


double FullSystem::calcMEnergy()
{
    if ( setting_forceAceptStep )
    {
        return 0;
    }
    return ef->calcMEnergyF();

}


double FullSystem::calcLEnergy()
{
    if ( setting_forceAceptStep )
    {
        return 0;
    }

    double Ef = ef->calcLEnergyF_MT();
    return Ef;

}


void FullSystem::removeOutliers()
{
    int numPointsDropped=0;
    for ( FrameHessian* fh : frameHessians )
    {
        for ( unsigned int i=0; i<fh->pointHessians.size(); i++ )
        {
            PointHessian* ph = fh->pointHessians[i];
            if ( ph==0 )
            {
                continue;
            }

            if ( ph->residuals.size() == 0 )
            {
                fh->pointHessiansOut.push_back ( ph );
                ph->efPoint->stateFlag = EFPointStatus::PS_DROP;
                fh->pointHessians[i] = fh->pointHessians.back();
                fh->pointHessians.pop_back();
                i--;
                numPointsDropped++;
            }
        }
    }
    ef->dropPointsF();
}




std::vector<VecX> FullSystem::getNullspaces (
    std::vector<VecX> &nullspaces_pose,
    std::vector<VecX> &nullspaces_scale,
    std::vector<VecX> &nullspaces_affA,
    std::vector<VecX> &nullspaces_affB )
{
    nullspaces_pose.clear();
    nullspaces_scale.clear();
    nullspaces_affA.clear();
    nullspaces_affB.clear();


    int n=CPARS+frameHessians.size() *8;
    std::vector<VecX> nullspaces_x0_pre;
    for ( int i=0; i<6; i++ )
    {
        VecX nullspace_x0 ( n );
        nullspace_x0.setZero();
        for ( FrameHessian* fh : frameHessians )
        {
            nullspace_x0.segment<6> ( CPARS+fh->idx*8 ) = fh->nullspaces_pose.col ( i );
            nullspace_x0.segment<3> ( CPARS+fh->idx*8 ) *= SCALE_XI_TRANS_INVERSE;
            nullspace_x0.segment<3> ( CPARS+fh->idx*8+3 ) *= SCALE_XI_ROT_INVERSE;
        }
        nullspaces_x0_pre.push_back ( nullspace_x0 );
        nullspaces_pose.push_back ( nullspace_x0 );
    }
    for ( int i=0; i<2; i++ )
    {
        VecX nullspace_x0 ( n );
        nullspace_x0.setZero();
        for ( FrameHessian* fh : frameHessians )
        {
            nullspace_x0.segment<2> ( CPARS+fh->idx*8+6 ) = fh->nullspaces_affine.col ( i ).head<2>();
            nullspace_x0[CPARS+fh->idx*8+6] *= SCALE_A_INVERSE;
            nullspace_x0[CPARS+fh->idx*8+7] *= SCALE_B_INVERSE;
        }
        nullspaces_x0_pre.push_back ( nullspace_x0 );
        if ( i==0 )
        {
            nullspaces_affA.push_back ( nullspace_x0 );
        }
        if ( i==1 )
        {
            nullspaces_affB.push_back ( nullspace_x0 );
        }
    }

    VecX nullspace_x0 ( n );
    nullspace_x0.setZero();
    for ( FrameHessian* fh : frameHessians )
    {
        nullspace_x0.segment<6> ( CPARS+fh->idx*8 ) = fh->nullspaces_scale;
        nullspace_x0.segment<3> ( CPARS+fh->idx*8 ) *= SCALE_XI_TRANS_INVERSE;
        nullspace_x0.segment<3> ( CPARS+fh->idx*8+3 ) *= SCALE_XI_ROT_INVERSE;
    }
    nullspaces_x0_pre.push_back ( nullspace_x0 );
    nullspaces_scale.push_back ( nullspace_x0 );

    return nullspaces_x0_pre;
}

// --------------------------------------------------------------------------------
// 次要的函数
void FullSystem::printOptRes ( const Vec3 &res, double resL, double resM, double resPrior, double LExact, float a, float b )
{
    printf ( "A(%f)=(AV %.3f). Num: A(%'d) + M(%'d); ab %f %f!\n",
             res[0],
             sqrtf ( ( float ) ( res[0] / ( patternNum*ef->resInA ) ) ),
             ef->resInA,
             ef->resInM,
             a,
             b
           );

}

void FullSystem::setPrecalcValues()
{
    for ( FrameHessian* fh : frameHessians )
    {
        fh->targetPrecalc.resize ( frameHessians.size() );
        for ( unsigned int i=0; i<frameHessians.size(); i++ )
        {
            fh->targetPrecalc[i].set ( fh, frameHessians[i], &Hcalib );
        }
    }

    ef->setDeltaF ( &Hcalib );
}

void FullSystem::setGammaFunction ( float* BInv )
{
    if ( BInv==0 )
    {
        return;
    }

    // copy BInv.
    memcpy ( Hcalib.Binv, BInv, sizeof ( float ) *256 );


    // invert.
    for ( int i=1; i<255; i++ )
    {
        // find val, such that Binv[val] = i.
        // I dont care about speed for this, so do it the stupid way.

        for ( int s=1; s<255; s++ )
        {
            if ( BInv[s] <= i && BInv[s+1] >= i )
            {
                Hcalib.B[i] = s+ ( i - BInv[s] ) / ( BInv[s+1]-BInv[s] );
                break;
            }
        }
    }
    Hcalib.B[0] = 0;
    Hcalib.B[255] = 255;
}

void FullSystem::printLogLine()
{
    if ( frameHessians.size() ==0 )
    {
        return;
    }

    if ( !setting_debugout_runquiet )
        printf ( "LOG %d: %.3f fine. Res: %d A, %d L, %d M; (%'d / %'d) forceDrop. a=%f, b=%f. Window %d (%d)\n",
                 allKeyFramesHistory.back()->id,
                 statistics_lastFineTrackRMSE,
                 ef->resInA,
                 ef->resInL,
                 ef->resInM,
                 ( int ) statistics_numForceDroppedResFwd,
                 ( int ) statistics_numForceDroppedResBwd,
                 allKeyFramesHistory.back()->aff_g2l.a,
                 allKeyFramesHistory.back()->aff_g2l.b,
                 frameHessians.back()->shell->id - frameHessians.front()->shell->id,
                 ( int ) frameHessians.size() );

    if ( !setting_logStuff )
    {
        return;
    }

    if ( numsLog != 0 )
    {
        ( *numsLog ) << allKeyFramesHistory.back()->id << " "  <<
                     statistics_lastFineTrackRMSE << " "  <<
                     ( int ) statistics_numCreatedPoints << " "  <<
                     ( int ) statistics_numActivatedPoints << " "  <<
                     ( int ) statistics_numDroppedPoints << " "  <<
                     ( int ) statistics_lastNumOptIts << " "  <<
                     ef->resInA << " "  <<
                     ef->resInL << " "  <<
                     ef->resInM << " "  <<
                     statistics_numMargResFwd << " "  <<
                     statistics_numMargResBwd << " "  <<
                     statistics_numForceDroppedResFwd << " "  <<
                     statistics_numForceDroppedResBwd << " "  <<
                     frameHessians.back()->aff_g2l().a << " "  <<
                     frameHessians.back()->aff_g2l().b << " "  <<
                     frameHessians.back()->shell->id - frameHessians.front()->shell->id << " "  <<
                     ( int ) frameHessians.size() << " "  << "\n";
        numsLog->flush();
    }
}


void FullSystem::printEigenValLine()
{
    if ( !setting_logStuff )
    {
        return;
    }
    if ( ef->lastHS.rows() < 12 )
    {
        return;
    }


    MatXX Hp = ef->lastHS.bottomRightCorner ( ef->lastHS.cols()-CPARS,ef->lastHS.cols()-CPARS );
    MatXX Ha = ef->lastHS.bottomRightCorner ( ef->lastHS.cols()-CPARS,ef->lastHS.cols()-CPARS );
    int n = Hp.cols() /8;
    assert ( Hp.cols() %8==0 );

    // sub-select
    for ( int i=0; i<n; i++ )
    {
        MatXX tmp6 = Hp.block ( i*8,0,6,n*8 );
        Hp.block ( i*6,0,6,n*8 ) = tmp6;

        MatXX tmp2 = Ha.block ( i*8+6,0,2,n*8 );
        Ha.block ( i*2,0,2,n*8 ) = tmp2;
    }
    for ( int i=0; i<n; i++ )
    {
        MatXX tmp6 = Hp.block ( 0,i*8,n*8,6 );
        Hp.block ( 0,i*6,n*8,6 ) = tmp6;

        MatXX tmp2 = Ha.block ( 0,i*8+6,n*8,2 );
        Ha.block ( 0,i*2,n*8,2 ) = tmp2;
    }

    VecX eigenvaluesAll = ef->lastHS.eigenvalues().real();
    VecX eigenP = Hp.topLeftCorner ( n*6,n*6 ).eigenvalues().real();
    VecX eigenA = Ha.topLeftCorner ( n*2,n*2 ).eigenvalues().real();
    VecX diagonal = ef->lastHS.diagonal();

    std::sort ( eigenvaluesAll.data(), eigenvaluesAll.data() +eigenvaluesAll.size() );
    std::sort ( eigenP.data(), eigenP.data() +eigenP.size() );
    std::sort ( eigenA.data(), eigenA.data() +eigenA.size() );

    int nz = std::max ( 100,setting_maxFrames*10 );

    if ( eigenAllLog != 0 )
    {
        VecX ea = VecX::Zero ( nz );
        ea.head ( eigenvaluesAll.size() ) = eigenvaluesAll;
        ( *eigenAllLog ) << allKeyFramesHistory.back()->id << " " <<  ea.transpose() << "\n";
        eigenAllLog->flush();
    }
    if ( eigenALog != 0 )
    {
        VecX ea = VecX::Zero ( nz );
        ea.head ( eigenA.size() ) = eigenA;
        ( *eigenALog ) << allKeyFramesHistory.back()->id << " " <<  ea.transpose() << "\n";
        eigenALog->flush();
    }
    if ( eigenPLog != 0 )
    {
        VecX ea = VecX::Zero ( nz );
        ea.head ( eigenP.size() ) = eigenP;
        ( *eigenPLog ) << allKeyFramesHistory.back()->id << " " <<  ea.transpose() << "\n";
        eigenPLog->flush();
    }

    if ( DiagonalLog != 0 )
    {
        VecX ea = VecX::Zero ( nz );
        ea.head ( diagonal.size() ) = diagonal;
        ( *DiagonalLog ) << allKeyFramesHistory.back()->id << " " <<  ea.transpose() << "\n";
        DiagonalLog->flush();
    }

    if ( variancesLog != 0 )
    {
        VecX ea = VecX::Zero ( nz );
        ea.head ( diagonal.size() ) = ef->lastHS.inverse().diagonal();
        ( *variancesLog ) << allKeyFramesHistory.back()->id << " " <<  ea.transpose() << "\n";
        variancesLog->flush();
    }

    std::vector<VecX> &nsp = ef->lastNullspaces_forLogging;
    ( *nullspacesLog ) << allKeyFramesHistory.back()->id << " ";
    for ( unsigned int i=0; i<nsp.size(); i++ )
    {
        ( *nullspacesLog ) << nsp[i].dot ( ef->lastHS * nsp[i] ) << " " << nsp[i].dot ( ef->lastbS ) << " " ;
    }
    ( *nullspacesLog ) << "\n";
    nullspacesLog->flush();

}

void FullSystem::printFrameLifetimes()
{
    if ( !setting_logStuff )
    {
        return;
    }

    unique_lock<mutex> lock ( trackMutex );

    std::ofstream* lg = new std::ofstream();
    lg->open ( "logs/lifetimeLog.txt", std::ios::trunc | std::ios::out );
    lg->precision ( 15 );

    for ( FrameShell* s : allFrameHistory )
    {
        ( *lg ) << s->id
                << " " << s->marginalizedAt
                << " " << s->statistics_goodResOnThis
                << " " << s->statistics_outlierResOnThis
                << " " << s->movedByOpt;



        ( *lg ) << "\n";
    }
    lg->close();
    delete lg;
}

void FullSystem::printResult ( std::string file )
{
    unique_lock<mutex> lock ( trackMutex );
    unique_lock<mutex> crlock ( shellPoseMutex );

    std::ofstream myfile;
    myfile.open ( file.c_str() );
    myfile << std::setprecision ( 15 );

    for ( FrameShell* s : allFrameHistory )
    {
        if ( !s->poseValid )
        {
            continue;
        }

        if ( setting_onlyLogKFPoses && s->marginalizedAt == s->id )
        {
            continue;
        }

        myfile << s->timestamp <<
               " " << s->camToWorld.translation().transpose() <<
               " " << s->camToWorld.so3().unit_quaternion().x() <<
               " " << s->camToWorld.so3().unit_quaternion().y() <<
               " " << s->camToWorld.so3().unit_quaternion().z() <<
               " " << s->camToWorld.so3().unit_quaternion().w() << "\n";
    }
    myfile.close();
}

void FullSystem::flagPointsForRemoval()
{
    assert ( EFIndicesValid );

    std::vector<FrameHessian*> fhsToKeepPoints;
    std::vector<FrameHessian*> fhsToMargPoints;

    for ( int i= ( ( int ) frameHessians.size() )-1; i>=0 && i >= ( ( int ) frameHessians.size() ); i-- )
        if ( !frameHessians[i]->flaggedForMarginalization )
        {
            fhsToKeepPoints.push_back ( frameHessians[i] );
        }

    for ( int i=0; i< ( int ) frameHessians.size(); i++ )
        if ( frameHessians[i]->flaggedForMarginalization )
        {
            fhsToMargPoints.push_back ( frameHessians[i] );
        }

    int flag_oob=0, flag_in=0, flag_inin=0, flag_nores=0;

    for ( FrameHessian* host : frameHessians )      // go through all active frames
    {
        for ( unsigned int i=0; i<host->pointHessians.size(); i++ )
        {
            PointHessian* ph = host->pointHessians[i];
            if ( ph==0 )
            {
                continue;
            }

            if ( ph->idepth_scaled < 0 || ph->residuals.size() ==0 )
            {
                host->pointHessiansOut.push_back ( ph );
                ph->efPoint->stateFlag = EFPointStatus::PS_DROP;
                host->pointHessians[i]=0;
                flag_nores++;
            }
            else if ( ph->isOOB ( fhsToKeepPoints, fhsToMargPoints ) || host->flaggedForMarginalization )
            {
                flag_oob++;
                if ( ph->isInlierNew() )
                {
                    flag_in++;
                    int ngoodRes=0;
                    for ( PointFrameResidual* r : ph->residuals )
                    {
                        r->resetOOB();
                        r->linearize ( &Hcalib );
                        r->efResidual->isLinearized = false;
                        r->applyRes ( true );
                        if ( r->efResidual->isActive() )
                        {
                            r->efResidual->fixLinearizationF ( ef );
                            ngoodRes++;
                        }
                    }
                    if ( ph->idepth_hessian > setting_minIdepthH_marg )
                    {
                        flag_inin++;
                        ph->efPoint->stateFlag = EFPointStatus::PS_MARGINALIZE;
                        host->pointHessiansMarginalized.push_back ( ph );
                    }
                    else
                    {
                        ph->efPoint->stateFlag = EFPointStatus::PS_DROP;
                        host->pointHessiansOut.push_back ( ph );
                    }


                }
                else
                {
                    host->pointHessiansOut.push_back ( ph );
                    ph->efPoint->stateFlag = EFPointStatus::PS_DROP;
                }

                host->pointHessians[i]=0;
            }
        }

        for ( int i=0; i< ( int ) host->pointHessians.size(); i++ )
        {
            if ( host->pointHessians[i]==0 )
            {
                host->pointHessians[i] = host->pointHessians.back();
                host->pointHessians.pop_back();
                i--;
            }
        }
    }
}

void FullSystem::blockUntilMappingIsFinished()
{
    // 等待建图线程完成
    unique_lock<mutex> lock ( trackMapSyncMutex );
    runMapping = false;
    trackedFrameSignal.notify_all();
    lock.unlock();
    mappingThread.join();
}

void FullSystem::debugPlotTracking()
{
    if ( disableAllDisplay )
    {
        return;
    }
    if ( !setting_render_plotTrackingFull )
    {
        return;
    }
    int wh = hG[0]*wG[0];

    int idx=0;
    for ( FrameHessian* f : frameHessians )
    {
        std::vector<MinimalImageB3* > images;

        // make images for all frames. will be deleted by the FrameHessian's destructor.
        for ( FrameHessian* f2 : frameHessians )
            if ( f2->debugImage == 0 )
            {
                f2->debugImage = new MinimalImageB3 ( wG[0], hG[0] );
            }

        for ( FrameHessian* f2 : frameHessians )
        {
            MinimalImageB3* debugImage=f2->debugImage;
            images.push_back ( debugImage );

            Eigen::Vector3f* fd = f2->dI;

            Vec2 affL = AffLight::fromToVecExposure ( f2->ab_exposure, f->ab_exposure, f2->aff_g2l(), f->aff_g2l() );

            for ( int i=0; i<wh; i++ )
            {
                // BRIGHTNESS TRANSFER
                float colL = affL[0] * fd[i][0] + affL[1];
                if ( colL<0 )
                {
                    colL=0;
                }
                if ( colL>255 )
                {
                    colL =255;
                }
                debugImage->at ( i ) = Vec3b ( colL, colL, colL );
            }
        }


        for ( PointHessian* ph : f->pointHessians )
        {
            assert ( ph->status == PointHessian::ACTIVE );
            if ( ph->status == PointHessian::ACTIVE || ph->status == PointHessian::MARGINALIZED )
            {
                for ( PointFrameResidual* r : ph->residuals )
                {
                    r->debugPlot();
                }
                f->debugImage->setPixel9 ( ph->u+0.5, ph->v+0.5, makeRainbow3B ( ph->idepth_scaled ) );
            }
        }


        char buf[100];
        snprintf ( buf, 100, "IMG %d", idx );
        IOWrap::displayImageStitch ( buf, images );
        idx++;
    }

    IOWrap::waitKey ( 0 );

}


void FullSystem::debugPlot ( std::string name )
{
    if ( disableAllDisplay )
    {
        return;
    }
    if ( !setting_render_renderWindowFrames )
    {
        return;
    }
    std::vector<MinimalImageB3* > images;

    float minID=0, maxID=0;
    if ( ( int ) ( freeDebugParam5+0.5f ) == 7 || ( debugSaveImages&&false ) )
    {
        std::vector<float> allID;
        for ( unsigned int f=0; f<frameHessians.size(); f++ )
        {
            for ( PointHessian* ph : frameHessians[f]->pointHessians )
                if ( ph!=0 )
                {
                    allID.push_back ( ph->idepth_scaled );
                }

            for ( PointHessian* ph : frameHessians[f]->pointHessiansMarginalized )
                if ( ph!=0 )
                {
                    allID.push_back ( ph->idepth_scaled );
                }

            for ( PointHessian* ph : frameHessians[f]->pointHessiansOut )
                if ( ph!=0 )
                {
                    allID.push_back ( ph->idepth_scaled );
                }
        }
        std::sort ( allID.begin(), allID.end() );
        int n = allID.size()-1;
        minID = allID[ ( int ) ( n*0.05 )];
        maxID = allID[ ( int ) ( n*0.95 )];


        // slowly adapt: change by maximum 10% of old span.
        float maxChange = 0.1* ( maxIdJetVisDebug - minIdJetVisDebug );
        if ( maxIdJetVisDebug < 0  || minIdJetVisDebug < 0 )
        {
            maxChange = 1e5;
        }


        if ( minID < minIdJetVisDebug - maxChange )
        {
            minID = minIdJetVisDebug - maxChange;
        }
        if ( minID > minIdJetVisDebug + maxChange )
        {
            minID = minIdJetVisDebug + maxChange;
        }


        if ( maxID < maxIdJetVisDebug - maxChange )
        {
            maxID = maxIdJetVisDebug - maxChange;
        }
        if ( maxID > maxIdJetVisDebug + maxChange )
        {
            maxID = maxIdJetVisDebug + maxChange;
        }

        maxIdJetVisDebug = maxID;
        minIdJetVisDebug = minID;

    }

    int wh = hG[0]*wG[0];
    for ( unsigned int f=0; f<frameHessians.size(); f++ )
    {
        MinimalImageB3* img = new MinimalImageB3 ( wG[0],hG[0] );
        images.push_back ( img );
        //float* fd = frameHessians[f]->I;
        Eigen::Vector3f* fd = frameHessians[f]->dI;


        for ( int i=0; i<wh; i++ )
        {
            int c = fd[i][0]*0.9f;
            if ( c>255 )
            {
                c=255;
            }
            img->at ( i ) = Vec3b ( c,c,c );
        }

        if ( ( int ) ( freeDebugParam5+0.5f ) == 0 )
        {
            for ( PointHessian* ph : frameHessians[f]->pointHessians )
            {
                if ( ph==0 )
                {
                    continue;
                }

                img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, makeRainbow3B ( ph->idepth_scaled ) );
            }
            for ( PointHessian* ph : frameHessians[f]->pointHessiansMarginalized )
            {
                if ( ph==0 )
                {
                    continue;
                }
                img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, makeRainbow3B ( ph->idepth_scaled ) );
            }
            for ( PointHessian* ph : frameHessians[f]->pointHessiansOut )
            {
                img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 255,255,255 ) );
            }
        }
        else if ( ( int ) ( freeDebugParam5+0.5f ) == 1 )
        {
            for ( PointHessian* ph : frameHessians[f]->pointHessians )
            {
                if ( ph==0 )
                {
                    continue;
                }
                img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, makeRainbow3B ( ph->idepth_scaled ) );
            }

            for ( PointHessian* ph : frameHessians[f]->pointHessiansMarginalized )
            {
                img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 0,0,0 ) );
            }

            for ( PointHessian* ph : frameHessians[f]->pointHessiansOut )
            {
                img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 255,255,255 ) );
            }
        }
        else if ( ( int ) ( freeDebugParam5+0.5f ) == 2 )
        {

        }
        else if ( ( int ) ( freeDebugParam5+0.5f ) == 3 )
        {
            for ( ImmaturePoint* ph : frameHessians[f]->immaturePoints )
            {
                if ( ph==0 )
                {
                    continue;
                }
                if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_GOOD ||
                        ph->lastTraceStatus==ImmaturePointStatus::IPS_SKIPPED ||
                        ph->lastTraceStatus==ImmaturePointStatus::IPS_BADCONDITION )
                {
                    if ( !std::isfinite ( ph->idepth_max ) )
                    {
                        img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 0,0,0 ) );
                    }
                    else
                    {
                        img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, makeRainbow3B ( ( ph->idepth_min + ph->idepth_max ) *0.5f ) );
                    }
                }
            }
        }
        else if ( ( int ) ( freeDebugParam5+0.5f ) == 4 )
        {
            for ( ImmaturePoint* ph : frameHessians[f]->immaturePoints )
            {
                if ( ph==0 )
                {
                    continue;
                }

                if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_GOOD )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 0,255,0 ) );
                }
                if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_OOB )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 255,0,0 ) );
                }
                if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_OUTLIER )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 0,0,255 ) );
                }
                if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_SKIPPED )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 255,255,0 ) );
                }
                if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_BADCONDITION )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 255,255,255 ) );
                }
                if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_UNINITIALIZED )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 0,0,0 ) );
                }
            }
        }
        else if ( ( int ) ( freeDebugParam5+0.5f ) == 5 )
        {
            for ( ImmaturePoint* ph : frameHessians[f]->immaturePoints )
            {
                if ( ph==0 )
                {
                    continue;
                }

                if ( ph->lastTraceStatus==ImmaturePointStatus::IPS_UNINITIALIZED )
                {
                    continue;
                }
                float d = freeDebugParam1 * ( sqrtf ( ph->quality )-1 );
                if ( d<0 )
                {
                    d=0;
                }
                if ( d>1 )
                {
                    d=1;
                }
                img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 0,d*255, ( 1-d ) *255 ) );
            }

        }
        else if ( ( int ) ( freeDebugParam5+0.5f ) == 6 )
        {
            for ( PointHessian* ph : frameHessians[f]->pointHessians )
            {
                if ( ph==0 )
                {
                    continue;
                }
                if ( ph->my_type==0 )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 255,0,255 ) );
                }
                if ( ph->my_type==1 )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 255,0,0 ) );
                }
                if ( ph->my_type==2 )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 0,0,255 ) );
                }
                if ( ph->my_type==3 )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 0,255,255 ) );
                }
            }
            for ( PointHessian* ph : frameHessians[f]->pointHessiansMarginalized )
            {
                if ( ph==0 )
                {
                    continue;
                }
                if ( ph->my_type==0 )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 255,0,255 ) );
                }
                if ( ph->my_type==1 )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 255,0,0 ) );
                }
                if ( ph->my_type==2 )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 0,0,255 ) );
                }
                if ( ph->my_type==3 )
                {
                    img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 0,255,255 ) );
                }
            }

        }
        if ( ( int ) ( freeDebugParam5+0.5f ) == 7 )
        {
            for ( PointHessian* ph : frameHessians[f]->pointHessians )
            {
                img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, makeJet3B ( ( ph->idepth_scaled-minID ) / ( ( maxID-minID ) ) ) );
            }
            for ( PointHessian* ph : frameHessians[f]->pointHessiansMarginalized )
            {
                if ( ph==0 )
                {
                    continue;
                }
                img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 0,0,0 ) );
            }
        }
    }
    IOWrap::displayImageStitch ( name.c_str(), images );
    IOWrap::waitKey ( 5 );

    for ( unsigned int i=0; i<images.size(); i++ )
    {
        delete images[i];
    }

    if ( ( debugSaveImages&&false ) )
    {
        for ( unsigned int f=0; f<frameHessians.size(); f++ )
        {
            MinimalImageB3* img = new MinimalImageB3 ( wG[0],hG[0] );
            Eigen::Vector3f* fd = frameHessians[f]->dI;

            for ( int i=0; i<wh; i++ )
            {
                int c = fd[i][0]*0.9f;
                if ( c>255 )
                {
                    c=255;
                }
                img->at ( i ) = Vec3b ( c,c,c );
            }

            for ( PointHessian* ph : frameHessians[f]->pointHessians )
            {
                img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, makeJet3B ( ( ph->idepth_scaled-minID ) / ( ( maxID-minID ) ) ) );
            }
            for ( PointHessian* ph : frameHessians[f]->pointHessiansMarginalized )
            {
                if ( ph==0 )
                {
                    continue;
                }
                img->setPixelCirc ( ph->u+0.5f, ph->v+0.5f, Vec3b ( 0,0,0 ) );
            }

            char buf[1000];
            snprintf ( buf, 1000, "images_out/kf_%05d_%05d_%02d.png",
                       frameHessians.back()->shell->id,  frameHessians.back()->frameID, f );
            IOWrap::writeImage ( buf,img );

            delete img;
        }
    }
}

void FullSystem::flagFramesForMarginalization ( FrameHessian* newFH )
{
    if ( setting_minFrameAge > setting_maxFrames )
    {
        for ( int i=setting_maxFrames; i< ( int ) frameHessians.size(); i++ )
        {
            FrameHessian* fh = frameHessians[i-setting_maxFrames];
            fh->flaggedForMarginalization = true;
        }
        return;
    }


    int flagged = 0;
    // marginalize all frames that have not enough points.
    for ( int i=0; i< ( int ) frameHessians.size(); i++ )
    {
        FrameHessian* fh = frameHessians[i];
        int in = fh->pointHessians.size() + fh->immaturePoints.size();
        int out = fh->pointHessiansMarginalized.size() + fh->pointHessiansOut.size();


        Vec2 refToFh=AffLight::fromToVecExposure ( frameHessians.back()->ab_exposure, fh->ab_exposure,
                     frameHessians.back()->aff_g2l(), fh->aff_g2l() );


        if ( ( in < setting_minPointsRemaining * ( in+out ) || fabs ( logf ( ( float ) refToFh[0] ) ) > setting_maxLogAffFacInWindow )
                && ( ( int ) frameHessians.size() )-flagged > setting_minFrames )
        {
            fh->flaggedForMarginalization = true;
            flagged++;
        }
        else
        {
        }
    }

    // marginalize one.
    if ( ( int ) frameHessians.size()-flagged >= setting_maxFrames )
    {
        double smallestScore = 1;
        FrameHessian* toMarginalize=0;
        FrameHessian* latest = frameHessians.back();


        for ( FrameHessian* fh : frameHessians )
        {
            if ( fh->frameID > latest->frameID-setting_minFrameAge || fh->frameID == 0 )
            {
                continue;
            }
            //if(fh==frameHessians.front() == 0) continue;

            double distScore = 0;
            for ( FrameFramePrecalc &ffh : fh->targetPrecalc )
            {
                if ( ffh.target->frameID > latest->frameID-setting_minFrameAge+1 || ffh.target == ffh.host )
                {
                    continue;
                }
                distScore += 1/ ( 1e-5+ffh.distanceLL );

            }
            distScore *= -sqrtf ( fh->targetPrecalc.back().distanceLL );


            if ( distScore < smallestScore )
            {
                smallestScore = distScore;
                toMarginalize = fh;
            }
        }

        toMarginalize->flaggedForMarginalization = true;
        flagged++;
    }

}

void FullSystem::marginalizeFrame ( FrameHessian* frame )
{
    // marginalize or remove all this frames points.

    assert ( ( int ) frame->pointHessians.size() ==0 );


    ef->marginalizeFrame ( frame->efFrame );

    // drop all observations of existing points in that frame.

    for ( FrameHessian* fh : frameHessians )
    {
        if ( fh==frame )
        {
            continue;
        }

        for ( PointHessian* ph : fh->pointHessians )
        {
            for ( unsigned int i=0; i<ph->residuals.size(); i++ )
            {
                PointFrameResidual* r = ph->residuals[i];
                if ( r->target == frame )
                {
                    if ( ph->lastResiduals[0].first == r )
                    {
                        ph->lastResiduals[0].first=0;
                    }
                    else if ( ph->lastResiduals[1].first == r )
                    {
                        ph->lastResiduals[1].first=0;
                    }


                    if ( r->host->frameID < r->target->frameID )
                    {
                        statistics_numForceDroppedResFwd++;
                    }
                    else
                    {
                        statistics_numForceDroppedResBwd++;
                    }

                    ef->dropResidual ( r->efResidual );
                    deleteOut<PointFrameResidual> ( ph->residuals,i );
                    break;
                }
            }
        }
    }

    {
        std::vector<FrameHessian*> v;
        v.push_back ( frame );
        for ( IOWrap::Output3DWrapper* ow : outputWrapper )
        {
            ow->publishKeyframes ( v, true, &Hcalib );
        }
    }


    frame->shell->marginalizedAt = frameHessians.back()->shell->id;
    frame->shell->movedByOpt = frame->w2c_leftEps().norm();

    deleteOutOrder<FrameHessian> ( frameHessians, frame );
    for ( unsigned int i=0; i<frameHessians.size(); i++ )
    {
        frameHessians[i]->idx = i;
    }
    setPrecalcValues();
    ef->setAdjointsF ( &Hcalib );
}



}
